

/* This is the main QuadRotor control program

*/

#include <Wire.h>
#include <EEPROM.h>
#include <SD.h>
#include <Compass.h>
#include <BMA180.h>
#include <ITG3200.h>
#include <BMP085.h>
#include <tinyOSC.h>
#include <FP_math.h>
#include <PIcontrol.h>
#include <TinyGPS.h>
#include <SimpleGPS.h>
#include <sonicRange.h>
#include <Servo.h>
#include <Quaternion.h>
#include <INU.h>

#define AxBIAS 87
#define AyBIAS -23
#define AzBIAS 207
#define AgXSHIFT 8
#define AgYSHIFT 7
#define AgZSHIFT 9

const int SERVO_MIN = 900;
const int SERVO_MAX = 1700;
const int SERVO_TEST = 1200;
boolean servoTest = false;
#define SERVO_1 38
#define SERVO_2 40
#define SERVO_3 42
#define SERVO_4 44
#define V_BATT 0
const int FILE_NUM_EE = 0;

File logFile;          // The SD log file object

Compass compass;
BMA180 bma180;
ITG3200 gyro;
BMP085 pAlt;

SimpleGPS gps;
tinyOSC osc;
sonicRange sx;
LongFFFilter rangeFilter;
int rangeRate = 0;
long sRange = 0;
long agl = 0;


INU inu;

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

int servo1_cmd;
int servo2_cmd;
int servo3_cmd;
int servo4_cmd;

boolean record = false;
boolean haveSD = false;          // Init SD success
boolean fileOpen = false;        // Have file open
boolean timeTagged = false;      // Wrote a time tag to the current file
String fileName = "";


const int dt = 20; // Update time, ms
int t_ctr = 0;
const int do_temp = 50;  // Number of pressure cycles between temp measurements
boolean t_run = false;
long next_time = 0;
int outCtr = 0;
boolean streamOut = false;

int tUpdate = 20;   // The actual number of millis since last update

Vector3 accel;
Vector3 gyroRate;
Vector3 gpsVelocity;
Vector3 bodyAccel;
Vector3 accelBias;
Vector3 accelInt;

int compass_head;
const int delta_head = 0;

int battery;

const char comma = ',';

/* ***************************************************************
Free Memory Function
****************************************************************/

extern void *__bss_end;
extern void *__brkval;
int get_free_memory()
{
  int free_memory;

  if((int)__brkval == 0)
    free_memory = ((int)&free_memory) - ((int)&__bss_end);
  else
    free_memory = ((int)&free_memory) - ((int)__brkval);

  return free_memory;
}



    
//*************************************************************************
void setup() {
  
  pinMode(2, INPUT);
 
  // Initialize serial ports 
  Serial.begin(9600);     // USB - debug port
  Serial1.begin(115200);  // WiFi - UDP/OSC port
  Serial2.begin(38400);   // GPS port
  Serial3.begin(9600);    // Sonic Proximity Sensor port
  
  servo1.attach(SERVO_1);
  servo1.writeMicroseconds(SERVO_MIN);
  servo1_cmd = SERVO_MIN;
  
  servo2.attach(SERVO_2);
  servo2.writeMicroseconds(SERVO_MIN);
  servo2_cmd = SERVO_MIN;
  
  servo3.attach(SERVO_3);
  servo3.writeMicroseconds(SERVO_MIN);
  servo3_cmd = SERVO_MIN;
  
  servo4.attach(SERVO_4);
  servo4.writeMicroseconds(SERVO_MIN);
  servo4_cmd = SERVO_MIN;
  
  // Initialize OSC object
  osc.begin(Serial1);
  
  // Initialize GPS object
  gps.begin(Serial2);
  // Initialize Sonic rangefinder
  sx.begin(Serial3);
  rangeFilter = LongFFFilter(509, 1396, 6);
  
   // Join I2C Bus
  Wire.begin();
  // Initialize the compass
  compass.begin();
  // Initialize the gyro
  gyro.begin();
  
  
  // Initialize pressure sensor
  pAlt.begin();
  delay(1000);
  pAlt.start_temp();
  delay(10);
  pAlt.get_temp();
  pAlt.start_pressure();
  delay(50);
  pAlt.pressure();
  pAlt.agl_0 = pAlt.altitude();
  
  bma180.x_bias = AyBIAS;
  bma180.y_bias = AxBIAS;
  bma180.z_bias = -AzBIAS;
  
  haveSD = SD.begin(10);
  
  inu = INU();
  
  resetINU();
  
  
}

//*************************************************************************
void loop() {
  
  int tCtr = millis() - next_time;
  
  if (tCtr >= 0) {
    
    tUpdate = tCtr + dt;
    next_time = millis() + dt;
    doControl();
    if (streamOut && (outCtr++ > 15)) {
      outCtr = 0;
      String s = "";
      imuErrStr(&s);
      Serial.print("Acc : ");
      Serial.println(s);
    }
  }
    
  if (sx.process()) {
    if (sx.range < 635) {
      // Have a sonic range value, set range & agl
      sRange = long(sx.range);
      pAlt.agl_0 = pAlt.altitude() - sRange;
    } else {
      // No sonic range - use pressure value
      sRange = pAlt.agl();
    }
  }
  
  gps.process();
  
  if (osc.available()) {
    char cmdChar = osc.method[0];
    processCmd(cmdChar);
  }
  
  if (Serial.available()) {
    //doControl();
    Serial.read();
    streamOut = !streamOut;
    
    
    /*
    String s = "";
    gyroStr(&s);
    Serial.print("Gyro: ");
    Serial.println(s);
    accelStr(&s);
    Serial.print("Accl: ");
    Serial.println(s);
    imuStr(&s);
    Serial.print("INU : ");
    Serial.println(s);
    imuAttStr(&s);
    Serial.print("Att : ");
    Serial.println(s);
    imuOffStr(&s);
    Serial.print("iOff: ");
    Serial.println(s);
    /*matStr(&s);
    Serial.print("Matx: ");
    Serial.println(s);
    imuBodyStr(&s);
    Serial.print("Body: ");
    Serial.println(s);
    imuInitStr(&s);
    Serial.print("Init: ");
    Serial.println(s);
    
    imuErrStr(&s);
    Serial.print("Acc : ");
    Serial.println(s);
    
    
    resetINU();
    
    
    imuBAStr(&s);
    Serial.print("Bacc: ");
    Serial.println(s);
    mathStr(&s);
    Serial.print("Math: ");
    Serial.println(s);
    */
  }

}
